#!/usr/bin/env python3
"""
Finger loading and point cloud generation utilities.

This module provides functionality to:
1. Load finger configurations from JSON files
2. Parse finger mesh files to extract geometry information
3. Generate point clouds on finger surfaces
4. Apply proper transformations based on finger configurations

The system supports the following embodiments:
- franka-panda (f)
- aloha-agilex (a)
- piper (p) 
- ur5-wsg (u)
- ARX-X5 (x)
"""
import numpy as np
import json
import os
import trimesh
import warnings
from pathlib import Path
from scipy.spatial.transform import Rotation
from typing import Dict, List, Tuple, Optional, Any
from scalePcd import scalePcd
from scipy.spatial.transform import Rotation
warnings.filterwarnings('ignore')

# Global cache for loaded finger data
_FINGER_CACHE = {}

# Paths
FINGER_CONFIG_PATH = str(Path(__file__).parent / "finger_mesh_config.json")

def get_all_attributes(obj, max_depth=2, current_depth=0):
    """
    Get all attributes of an unknown object recursively.
    
    Args:
        obj: The object to inspect
        max_depth: Maximum recursion depth to avoid infinite loops
        current_depth: Current recursion depth
        
    Returns:
        dict: Dictionary containing all attributes and their values/types
    """
    if obj is None:
        return {"type": "None", "value": None}
    
    if current_depth >= max_depth:
        return {"type": str(type(obj)), "value": "Max depth reached"}
    
    result = {
        "type": str(type(obj)),
        "attributes": {},
        "methods": {},
        "properties": {}
    }
    
    try:
        # Get all attributes
        for attr_name in dir(obj):
            if attr_name.startswith('_'):  # Skip private attributes
                continue
                
            try:
                attr_value = getattr(obj, attr_name)
                
                # Skip methods and functions
                if callable(attr_value):
                    result["methods"][attr_name] = {
                        "type": str(type(attr_value)),
                        "doc": getattr(attr_value, '__doc__', 'No docstring')
                    }
                    continue
                
                # Handle different types of attributes
                if isinstance(attr_value, (int, float, str, bool, type(None))):
                    result["attributes"][attr_name] = {
                        "type": str(type(attr_value)),
                        "value": attr_value
                    }
                elif isinstance(attr_value, (list, tuple)):
                    result["attributes"][attr_name] = {
                        "type": str(type(attr_value)),
                        "length": len(attr_value),
                        "sample": str(attr_value[:3]) if len(attr_value) > 0 else "empty"
                    }
                elif isinstance(attr_value, dict):
                    result["attributes"][attr_name] = {
                        "type": str(type(attr_value)),
                        "keys": list(attr_value.keys())[:5],  # Show first 5 keys
                        "length": len(attr_value)
                    }
                elif isinstance(attr_value, np.ndarray):
                    result["attributes"][attr_name] = {
                        "type": "numpy.ndarray",
                        "shape": attr_value.shape,
                        "dtype": str(attr_value.dtype),
                        "sample": str(attr_value.flatten()[:5]) if attr_value.size > 0 else "empty"
                    }
                else:
                    # For complex objects, try to get basic info
                    try:
                        result["attributes"][attr_name] = {
                            "type": str(type(attr_value)),
                            "str_repr": str(attr_value)[:100] + "..." if len(str(attr_value)) > 100 else str(attr_value)
                        }
                    except:
                        result["attributes"][attr_name] = {
                            "type": str(type(attr_value)),
                            "value": "Could not get string representation"
                        }
                        
            except Exception as e:
                result["attributes"][attr_name] = {
                    "type": "Error",
                    "error": str(e)
                }
                
    except Exception as e:
        result["error"] = f"Error inspecting object: {str(e)}"
    
    return result

def print_object_info(obj, name="object"):
    """
    Print comprehensive information about an object in a readable format.
    
    Args:
        obj: The object to inspect
        name: Name of the object for display purposes
    """
    print(f"\n=== {name} Information ===")
    info = get_all_attributes(obj)
    
    print(f"Type: {info['type']}")
    
    if "error" in info:
        print(f"Error: {info['error']}")
        return
    
    if info["attributes"]:
        print(f"\nAttributes ({len(info['attributes'])}):")
        for attr_name, attr_info in info["attributes"].items():
            print(f"  {attr_name}: {attr_info}")
    
    if info["methods"]:
        print(f"\nMethods ({len(info['methods'])}):")
        for method_name, method_info in info["methods"].items():
            print(f"  {method_name}: {method_info['type']}")
            if method_info['doc']:
                doc_lines = method_info['doc'].strip().split('\n')
                for line in doc_lines[:2]:  # Show first 2 lines of docstring
                    print(f"    {line}")
    
    if info["properties"]:
        print(f"\nProperties ({len(info['properties'])}):")
        for prop_name, prop_info in info["properties"].items():
            print(f"  {prop_name}: {prop_info}")

class FingerLoader:
    def __init__(self):
        self.config = None
        self.left_finger_mesh = None
        self.right_finger_mesh = None
        self.left_finger_mesh_path = None
        self.right_finger_mesh_path = None
        
    def load_finger_config(self, embodiment_name: str) -> Dict[str, List[Dict]]:
        """
        从 JSON 文件加载手指配置信息
        
        Args:
            embodiment_name: 机器人名称，如 "franka-panda"
            
        Returns:
            包含手指配置的字典，格式为:
            {
                "left_finger": [{"path": "/path/to/mesh.glb", "rpy": [0, 0, 0]}, ...],
                "right_finger": [{"path": "/path/to/mesh.glb", "rpy": [0, 0, 0]}, ...]
            }
        """
        config_path = Path(FINGER_CONFIG_PATH)
        assert config_path.exists(), f"Finger config file not found: {config_path}"
       
        with open(config_path, "r") as f:
            config = json.load(f)
        
        return config.get(embodiment_name, {"left_finger": [], "right_finger": []})
    
    def get_finger_mesh_info(self, finger_config: Dict, finger_side: str = "left") -> Tuple[str, np.ndarray]:
        """
        获取手指网格路径和旋转信息
        
        Args:
            finger_config: 手指配置字典
            finger_side: "left" 或 "right"
            
        Returns:
            (mesh_path, rpy_rotation): 网格路径和旋转角度
        """
        finger_key = f"{finger_side}_finger"
        if finger_key not in finger_config or not finger_config[finger_key]:
            raise ValueError(f"No configuration found for {finger_key}")
        
        # 默认使用第一个配置
        config = finger_config[finger_key][0]
        mesh_path = config["path"]
        rpy = np.array(config["rpy"])
        
        return mesh_path, rpy
    
    def load_finger_mesh(self, finger_side: str = "left"):
        """Load the finger mesh from GLB file and cache it."""
        if finger_side == "left":
            if self.left_finger_mesh is None:
                assert os.path.exists(self.left_finger_mesh_path), f"Left finger mesh file not found: {self.left_finger_mesh_path}"
                loaded_mesh = trimesh.load(self.left_finger_mesh_path)
                
                # Handle case where GLB file loads as a Scene (containing multiple meshes)
                if isinstance(loaded_mesh, trimesh.Scene):
                    # Get all meshes from the scene
                    meshes = list(loaded_mesh.geometry.values())
                    if len(meshes) == 0:
                        raise ValueError(f"No meshes found in scene: {self.left_finger_mesh_path}")
                    elif len(meshes) == 1:
                        self.left_finger_mesh = meshes[0]
                    else:
                        # If multiple meshes, combine them into a single mesh
                        self.left_finger_mesh = trimesh.util.concatenate(meshes)
                else:
                    # If it's already a single mesh
                    self.left_finger_mesh = loaded_mesh
                    
                print(f"Successfully loaded left finger mesh from {self.left_finger_mesh_path}")
            return True
        else:  # right finger
            if self.right_finger_mesh is None:
                assert os.path.exists(self.right_finger_mesh_path), f"Right finger mesh file not found: {self.right_finger_mesh_path}"
                loaded_mesh = trimesh.load(self.right_finger_mesh_path)
                
                # Handle case where GLB file loads as a Scene (containing multiple meshes)
                if isinstance(loaded_mesh, trimesh.Scene):
                    # Get all meshes from the scene
                    meshes = list(loaded_mesh.geometry.values())
                    if len(meshes) == 0:
                        raise ValueError(f"No meshes found in scene: {self.right_finger_mesh_path}")
                    elif len(meshes) == 1:
                        self.right_finger_mesh = meshes[0]
                    else:
                        # If multiple meshes, combine them into a single mesh
                        self.right_finger_mesh = trimesh.util.concatenate(meshes)
                else:
                    # If it's already a single mesh
                    self.right_finger_mesh = loaded_mesh
                    
                print(f"Successfully loaded right finger mesh from {self.right_finger_mesh_path}")
            return True
    
    def get_mesh(self, finger_side: str = "left"):
        """Get the finger mesh for the specified side."""
        self.load_finger_mesh(finger_side)
        if finger_side == "left":
            return self.left_finger_mesh
        else:
            return self.right_finger_mesh
    
    def get_finger_bbox_info(self, mesh_path: str) -> Dict[str, np.ndarray]:
        """
        获取手指网格的边界框信息
        
        Args:
            mesh_path: 网格文件路径
            
        Returns:
            包含边界框信息的字典
        """
        if not os.path.exists(mesh_path):
            raise FileNotFoundError(f"Mesh file not found: {mesh_path}")
        
        mesh = trimesh.load(mesh_path, force='mesh')
        if isinstance(mesh, trimesh.Scene):
            meshes = list(mesh.geometry.values())
            if len(meshes) == 0:
                raise ValueError(f"No meshes found in scene: {mesh_path}")
            elif len(meshes) == 1:
                mesh = meshes[0]
            else:
                mesh = trimesh.util.concatenate(meshes)
        
        bbox = mesh.bounding_box
        bbox_dims = bbox.extents  # [width, height, depth]
        bbox_center = bbox.center
        bbox_vertices = bbox.vertices
        
        # 找到最小的两个面（不包括最长边的面）
        # 假设最长边是深度方向
        min_face_centers = []
        for i in range(len(bbox_vertices)):
            for j in range(i+1, len(bbox_vertices)):
                # 计算面的中心点
                face_center = (bbox_vertices[i] + bbox_vertices[j]) / 2
                min_face_centers.append(face_center)
        
        return {
            "dims": bbox_dims,
            "center": bbox_center,
            "vertices": bbox_vertices,
            "min_face_centers": min_face_centers
        }
    
    def calculate_finger_points(self, mesh_path: str, rpy: np.ndarray, 
                              attach_length: float = 0.05, 
                              end_length: float = 0.1) -> Dict[str, np.ndarray]:
        """
        计算手指的关键点位置
        
        Args:
            mesh_path: 网格文件路径
            rpy: 旋转角度 [roll, pitch, yaw]
            attach_length: 附着点距离
            end_length: 端点距离
            
        Returns:
            包含关键点位置的字典
        """
        bbox_info = self.get_finger_bbox_info(mesh_path)
        
        # 创建旋转矩阵
        rotation = Rotation.from_euler('xyz', rpy)
        rot_matrix = rotation.as_matrix()
        
        # Z+ 方向指向从附着点到端点的方向
        z_direction = rot_matrix @ np.array([0, 0, 1])
        
        # 计算附着点和端点位置
        attach_point = bbox_info["center"] + z_direction * attach_length
        end_point = bbox_info["center"] + z_direction * end_length
        
        return {
            "attach_point": attach_point,
            "end_point": end_point,
            "center": bbox_info["center"],
            "direction": z_direction,
            "bbox_dims": bbox_info["dims"]
        }
    
    def get_finger_geometry(self, embodiment_name: str, finger_side: str = "left") -> Dict[str, np.ndarray]:
        """
        获取手指几何信息的主函数
        
        Args:
            embodiment_name: 机器人名称
            finger_side: "left" 或 "right"
            
        Returns:
            包含手指几何信息的字典
        """
        finger_config = self.load_finger_config(embodiment_name)
        mesh_path, rpy = self.get_finger_mesh_info(finger_config, finger_side)
        
        # 使用默认长度，可以根据需要调整
        finger_points = self.calculate_finger_points(mesh_path, rpy)
        
        return {
            "mesh_path": mesh_path,
            "rpy": rpy,
            "attach_point": finger_points["attach_point"],
            "end_point": finger_points["end_point"],
            "center": finger_points["center"],
            "direction": finger_points["direction"],
            "bbox_dims": finger_points["bbox_dims"]
        }
    
    def reinit(self, embodiment_code: str):
        """Initialize the finger loader with embodiment configuration."""
        if self.config is None:
            # Load finger configuration based on embodiment code
            embodiment_name = self._get_embodiment_name_from_code(embodiment_code)
            finger_config = self.load_finger_config(embodiment_name)
            
            # Set mesh paths
            if finger_config.get("left_finger"):
                self.left_finger_mesh_path = Path(__file__).parent.parent /"RoboTwin" / finger_config["left_finger"][0]["path"]
            if finger_config.get("right_finger"):
                self.right_finger_mesh_path = Path(__file__).parent.parent /"RoboTwin" / finger_config["right_finger"][0]["path"]
            
            self.config = finger_config
    
    def _get_embodiment_name_from_code(self, embodiment_code: str) -> str:
        """Convert embodiment code to full name."""
        mapping = {
            'f': 'franka-panda',
            'a': 'aloha-agilex',
            'p': 'piper',
            'u': 'ur5-wsg',
            'x': 'ARX-X5'
        }
        return mapping.get(embodiment_code, 'franka-panda')
    
    def quat_to_rotmat(self, q):
        """Convert quaternion to rotation matrix."""
        w, x, y, z = q
        return np.array([
            [1-2*y*y-2*z*z, 2*x*y-2*z*w, 2*x*z+2*y*w],
            [2*x*y+2*z*w, 1-2*x*x-2*z*z, 2*y*z-2*x*w],
            [2*x*z-2*y*w, 2*y*z+2*x*w, 1-2*x*x-2*y*y]
        ])
    
    def get_real_finger_pcd(self, n: int, pose: np.ndarray, finger_side: str = "left") -> np.ndarray:
        """
        Generate point cloud from real finger mesh by sampling points on its surface.
        
        Args:
            n: Number of points to sample
            pose: 7D pose array [x, y, z, qw, qx, qy, qz]
            finger_side: "left" or "right"
            
        Returns:
            np.ndarray: Point cloud with colors, shape (n, 6) [x, y, z, r, g, b]
        """
        # Load the finger mesh if not already loaded
        self.load_finger_mesh(finger_side)
        
        # Get the appropriate mesh
        mesh = self.left_finger_mesh if finger_side == "left" else self.right_finger_mesh
        
        from trimesh.sample import sample_surface
        points, face_indices = sample_surface(mesh, n)
        
        # Get colors from the mesh
        if not (mesh is not None and 
            hasattr(mesh, 'visual') and 
            mesh.visual is not None):
            print_object_info(mesh, "finger_mesh")
            exit()
            
        visual = mesh.visual
        
        # If it's TextureVisuals, convert to ColorVisuals first
        if hasattr(visual, 'kind') and visual.kind == 'texture':
            color_visual = visual.to_color()
            
            if not hasattr(color_visual, 'vertex_colors'):
                print_object_info(color_visual, "color_visual")
                exit()
            
            vertex_colors = color_visual.vertex_colors
            face_vertices = mesh.faces[face_indices]
            colors = np.mean(vertex_colors[face_vertices], axis=1)[:, :3].astype(np.float32)
        
        # If it's already ColorVisuals, access vertex_colors directly
        elif hasattr(visual, 'vertex_colors'):
            vertex_colors = visual.vertex_colors
            face_vertices = mesh.faces[face_indices]
            colors = np.mean(vertex_colors[face_vertices], axis=1)[:, :3].astype(np.float32)
        else:
            print_object_info(visual, "visual")
            exit()
        
        # Apply pose transformation
        rot = self.quat_to_rotmat(pose[3:7])
        points_world = points @ rot.T + pose[0:3]
        
        # Combine points and colors
        pcd = np.concatenate([points_world, colors], axis=1)
        pcd = scalePcd(pcd, tag=f"get_real_finger_pcd_{finger_side}")
        return pcd
    
    def get_two_finger_pcd(self, finger_key: str, n1: int, n2: int, 
                          pose1: np.ndarray, pose2: np.ndarray) -> np.ndarray:
        """
        Generate point clouds for both left and right fingers.
        
        Args:
            finger_key: Finger identifier
            n1: Number of points for left finger
            n2: Number of points for right finger
            pose1: 7D pose for left finger [x, y, z, qw, qx, qy, qz]
            pose2: 7D pose for right finger [x, y, z, qw, qx, qy, qz]
            
        Returns:
            np.ndarray: Combined point cloud for both fingers
        """
        pcd1 = self.get_real_finger_pcd(n1, pose1, "left")
        pcd2 = self.get_real_finger_pcd(n2, pose2, "right")
        return np.concatenate([pcd1, pcd2], axis=0)

# Global finger loader instance
finger_loader = FingerLoader()

def get_two_finger_pcd(finger_key, n1, n2, pose1, pose2):
    """
    Global function to get point clouds for both fingers.
    Interface similar to loadGripper.py
    """
    finger_loader.reinit(finger_key)
    return finger_loader.get_two_finger_pcd(finger_key, n1, n2, pose1, pose2)

def get_finger_geometry(embodiment_name: str, finger_side: str = "left") -> Dict[str, np.ndarray]:
    """
    Global function to get finger geometry information.
    Interface similar to loadGripper.py
    """
    finger_loader.reinit(embodiment_name)
    return finger_loader.get_finger_geometry(embodiment_name, finger_side)

def get_4finger_from_gripper36(finger_key: str, n: int, gripper36: np.ndarray) -> np.ndarray:
    """
    Generate point clouds for 4 fingers from gripper36 data.
    
    Args:
        finger_key: Finger identifier
        n: Number of points to generate for 4 fingers (should be 10% of total)
        gripper36: 36D vector containing gripper and finger information
                  Format: ((x y z r p y)gripper + (x,y,z)*4 finger key pos)*2
                  [0:6] - left gripper pos+rot
                  [6:18] - left gripper 4 finger points:
                    [6:9] - leftFingerAttach (left gripper's left finger attach)
                    [9:12] - rightFingerAttach (left gripper's right finger attach)
                    [12:15] - leftFingerEnd (left gripper's left finger end)
                    [15:18] - rightFingerEnd (left gripper's right finger end)
                  [18:24] - right gripper pos+rot  
                  [24:36] - right gripper 4 finger points:
                    [24:27] - leftFingerAttach (right gripper's left finger attach)
                    [27:30] - rightFingerAttach (right gripper's right finger attach)
                    [30:33] - leftFingerEnd (right gripper's left finger end)
                    [33:36] - rightFingerEnd (right gripper's right finger end)
    
    Note:
        Each finger's pose is calculated from its own attach and end points:
        - The finger mesh is a cuboid with 6 faces (2 smallest faces have same area)
        - The finger's tip (end of longest dimension) aligns with the end point
        - The finger's base (start of longest dimension) aligns with the attach point
        - The finger's center position is the midpoint between attach and end points
        - The finger's longest dimension aligns with the direction from end to attach
        - Z+ axis points from end to attach point (same as JSON finger_mesh_config RPY)
        - This is different from using the global gripper RPY
    
    Returns:
        np.ndarray: Point cloud for 4 fingers, shape (n_finger_points, 6) [x, y, z, r, g, b]
    """
    # Initialize finger loader
    finger_loader.reinit(finger_key)
    
    # Equal distribution among 4 fingers
    n_per_finger = n // 4  # Equal distribution among 4 fingers
    
    # Extract finger positions from gripper36
    # Structure: ((x y z r p y)gripper + (x,y,z)*2 finger points)*2
    # [0:6] - left gripper pos+rot
    # [6:12] - left gripper left finger (attach, end) - 2 points * 3 coords
    # [12:18] - left gripper right finger (attach, end) - 2 points * 3 coords  
    # [18:24] - right gripper pos+rot
    # [24:30] - right gripper left finger (attach, end) - 2 points * 3 coords
    # [30:36] - right gripper right finger (attach, end) - 2 points * 3 coords
    
    left_finger_points = gripper36[6:18].reshape(4, 3)  # 4 finger points for left gripper (2 fingers * 2 points each)
    right_finger_points = gripper36[24:36].reshape(4, 3)  # 4 finger points for right gripper (2 fingers * 2 points each)
    
    finger_pcds = []
    
    # Define finger configurations with their specific attach/end points
    # Each finger uses its own attach and end points from the gripper36 data
    # left_finger_points: [0] = leftFingerAttach, [1] = leftFingerEnd, [2] = rightFingerAttach, [3] = rightFingerEnd
    # right_finger_points: [0] = leftFingerAttach, [1] = leftFingerEnd, [2] = rightFingerAttach, [3] = rightFingerEnd
    finger_configs = [
        # Left gripper left finger: uses leftFingerAttach[0] and leftFingerEnd[1]
        ("left", left_finger_points[0:2]),   # Left gripper left finger
        # Left gripper right finger: uses rightFingerAttach[2] and rightFingerEnd[3]  
        ("left", left_finger_points[2:4]),  # Left gripper right finger
        # Right gripper left finger: uses leftFingerAttach[0] and leftFingerEnd[1]
        ("right", right_finger_points[0:2]), # Right gripper left finger
        # Right gripper right finger: uses rightFingerAttach[2] and rightFingerEnd[3]
        ("right", right_finger_points[2:4]) # Right gripper right finger
    ]
    
    for gripper_side, finger_points in finger_configs:
        attach_point, end_point = finger_points[0], finger_points[1]
        # Calculate finger pose based on its own attach and end points
        # The finger mesh is a cuboid with 6 faces
        # We need to align one of the two smallest faces with the end point
        # and the other smallest face should be close to the attach point
        
        # Get finger mesh to calculate its bounding box
        finger_mesh = finger_loader.get_mesh(gripper_side)
        bbox = finger_mesh.bounding_box
        bbox_extents = bbox.extents  # [width, height, depth] - sorted by size
        
        # Find the longest dimension (Z+ direction)
        longest_dim_idx = np.argmax(bbox_extents)
        longest_dim_length = bbox_extents[longest_dim_idx]
        
        # The finger direction should align with the longest dimension
        finger_direction = attach_point - end_point  # Vector from end to attach
        finger_direction = finger_direction / np.linalg.norm(finger_direction)
        
        # Calculate finger position so that:
        # 1. The finger's tip (end of longest dimension) aligns with end_point
        # 2. The finger's base (start of longest dimension) aligns with attach_point
        # 3. The finger's longest dimension aligns with the direction
        
        # Position the finger so that its tip matches end_point
        # and its base is at attach_point
        # Since get_real_finger_pcd uses pose as mesh center, we need to calculate
        # the center position between attach_point and end_point
        finger_position = (attach_point + end_point) / 2
        print(f"finger_position (center): {finger_position}")
        print(f"end_point: {end_point}")
        print(f"finger_direction: {finger_direction}")
        print(f"longest_dim_length: {longest_dim_length}")
        print(f"attach_point: {attach_point}")
        # Create rotation matrix where Z+ aligns with finger direction
        z_axis = finger_direction
        # Find a perpendicular vector for x_axis
        if abs(z_axis[0]) < abs(z_axis[1]):
            x_axis = np.array([1, 0, 0])
        else:
            x_axis = np.array([0, 1, 0])
        x_axis = x_axis - np.dot(x_axis, z_axis) * z_axis
        x_axis = x_axis / np.linalg.norm(x_axis)
        # y_axis = z_axis × x_axis
        y_axis = np.cross(z_axis, x_axis)
        
        # Create rotation matrix
        rot_matrix = np.column_stack([x_axis, y_axis, z_axis])
        
        # Convert to quaternion
        finger_quat = Rotation.from_matrix(rot_matrix).as_quat()
        
        # Create 7D pose for this finger
        finger_pose_7d = np.concatenate([finger_position, finger_quat])
        
        # Generate point cloud for this finger
        finger_pcd = finger_loader.get_real_finger_pcd(n_per_finger, finger_pose_7d, gripper_side)
        finger_pcds.append(finger_pcd)
    
    # Combine all finger point clouds
    if finger_pcds:
        combined_pcd = np.concatenate(finger_pcds, axis=0)
        
        # DEBUG: Add bbox vertices and center points for visualization
        debug_points = []
        for gripper_side, finger_points in finger_configs:
            attach_point, end_point = finger_points[0], finger_points[1]
            
            # Get finger mesh and its bbox
            finger_mesh = finger_loader.get_mesh(gripper_side)
            bbox = finger_mesh.bounding_box
            
            # Calculate finger pose (same as above)
            finger_direction = attach_point - end_point
            finger_direction = finger_direction / np.linalg.norm(finger_direction)
            finger_position = (attach_point + end_point) / 2
            
            # Create rotation matrix
            z_axis = finger_direction
            if abs(z_axis[0]) < abs(z_axis[1]):
                x_axis = np.array([1, 0, 0])
            else:
                x_axis = np.array([0, 1, 0])
            x_axis = x_axis - np.dot(x_axis, z_axis) * z_axis
            x_axis = x_axis / np.linalg.norm(x_axis)
            y_axis = np.cross(z_axis, x_axis)
            rot_matrix = np.column_stack([x_axis, y_axis, z_axis])
            
            # Transform bbox vertices to world coordinates
            bbox_vertices = bbox.vertices  # 8 vertices
            bbox_center = bbox.centroid      # 1 center point
            
            # Apply transformation to vertices
            vertices_world = bbox_vertices @ rot_matrix.T + finger_position
            center_world = bbox_center @ rot_matrix.T + finger_position
            
            # Add cyan color for bbox vertices [0, 1, 1]
            cyan_vertices = np.concatenate([vertices_world, np.tile([0, 255, 255], (8, 1))], axis=1)
            debug_points.append(cyan_vertices)
            
            # Add purple color for center point [1, 0, 1]
            purple_center = np.concatenate([center_world.reshape(1, 3), np.array([[255, 0, 255]])], axis=1)
            debug_points.append(purple_center)
        
        # Combine debug points with finger point clouds
        if debug_points:
            debug_pcd = np.concatenate(debug_points, axis=0)
            print(f"debug_pcd.shape: {debug_pcd.shape}")
            combined_pcd = np.concatenate([combined_pcd, debug_pcd], axis=0)
            print(f"combined_pcd.shape: {combined_pcd.shape}")
        
        return combined_pcd
    else:
        return np.array([]).reshape(0, 6)

if __name__ == "__main__":
    # 测试代码
    try:
        finger_info = get_finger_geometry("franka-panda", "left")
        print("Left finger info:", finger_info)
    except Exception as e:
        print(f"Test failed: {e}")